﻿#include "SPP.h"
#include <cmath>
#include <stdexcept>
#include <iostream>

SPPSolver::SPPSolver(
    CoordSys* coordSys,
    GNSSSatPos* satPosCalculator,
    IonoCrr* ionoCorrector,
    TropCrr* tropCorrector,
    ReadO* obsReader,
    ReadN* navReader,
    TimeSys* timeSys
) :
    mCoordSys(coordSys),
    mSatPosCalculator(satPosCalculator),
    mIonoCorrector(ionoCorrector),
    mTropCorrector(tropCorrector),
    mObsReader(obsReader),
    mNavReader(navReader),
    mTimeSys(timeSys)
{}

bool SPPSolver::solve_epoch(int epochIndex, XYZ& receiverPos, double& clockBias) {
    // 1. 获取历元观测数据
    const vector<obsEpoch>* obsData = mObsReader->get_obs();
    if (epochIndex < 0 || epochIndex >= obsData->size()) {
        throw std::out_of_range("历元索引超出范围");
    }

    const obsEpoch& epoch = obsData->at(epochIndex);
    BaseT obsTime = mTimeSys->str2baseT(epoch.time);

    // 2. 初始化迭代变量
    XYZ currentPos = receiverPos;
    double currentClockBias = clockBias;
    int iteration = 0;
    bool converged = false;

    while (iteration < MAX_ITERATIONS && !converged) {
        // 检查可用卫星数是否足够（至少4颗）
        int numSats = epoch.listObsSat.size();
        if (numSats < 4) {
            std::cerr << "可用卫星数不足（需要至少4颗），当前：" << numSats << std::endl;
            return false;
        }

        Matrix designMatrix(numSats, 4);
        Matrix obsVector(numSats, 1);

        for (int i = 0; i < numSats; i++) {
            const obsSat& sat = epoch.listObsSat[i];

            BroadEphemeris* eph = mNavReader->getEphemeris(sat.satID, obsTime);
            if (!eph) continue;

            XYZ satPos = mSatPosCalculator->get_satPos(sat.satID, obsTime);
            double satClockBias = eph->clockBias * SPEED_OF_LIGHT;

            // 计算几何距离（添加防零值检查）
            XYZ satPosDiff = mCoordSys->subtract(satPos, currentPos);
            double geoDist = mCoordSys->mod(satPosDiff);
            if (geoDist < 1e-9) {
                std::cerr << "卫星与接收机位置重合，几何距离异常" << std::endl;
                continue;
            }

            Azel azel = mCoordSys->get_azel(currentPos, satPos);
            BLH blh = mCoordSys->XYZ2BLH(currentPos);

            double ionoCorr = mIonoCorrector->get_ionoCorr(blh, azel, mTimeSys->calend2sow(mTimeSys->baseT2calend(obsTime)));
            double tropCorr = mTropCorrector->get_tropCorr(blh, azel.ele);

            // 计算方向余弦（使用已计算的差值向量）
            XYZ direction = satPosDiff;
            direction.x /= geoDist;
            direction.y /= geoDist;
            direction.z /= geoDist;

            designMatrix[i][0] = -direction.x;
            designMatrix[i][1] = -direction.y;
            designMatrix[i][2] = -direction.z;
            designMatrix[i][3] = 1.0;

            double correctedPr = sat.values[0] + satClockBias - ionoCorr - tropCorr;
            obsVector[i][0] = correctedPr - geoDist - currentClockBias;
        }

        try {
            Matrix A_T = designMatrix.transpose();
            Matrix N = A_T * designMatrix;
            Matrix U = A_T * obsVector;

            Matrix N_inv = N.invert();
            Matrix delta = N * U;  

            currentPos.x += delta[0][0];
            currentPos.y += delta[1][0];
            currentPos.z += delta[2][0];
            currentClockBias += delta[3][0];

            double posDelta = sqrt(delta[0][0]*delta[0][0] + delta[1][0]*delta[1][0] + delta[2][0]*delta[2][0]);

            if (posDelta < CONVERGENCE_THRESHOLD) {
                converged = true;
            }
        }
        catch (const std::exception& e) {
            std::cerr << "矩阵求逆失败（可能卫星几何构型差）: " << e.what() << std::endl;
            return false;
        }

        iteration++;
    }

    // 8. 更新结果
    receiverPos = currentPos;
    clockBias = currentClockBias;

    return converged;
}